#Created by 张荣开 on 25-6-1.

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
from yesense_interface.msg import EulerOnly # type: ignore
import time
import math

class PIDControllerNode(Node):
    def __init__(self):
        super().__init__('pid_controller_node')
        
        # ====================== PID基础参数配置 ======================
        self.kp = 0.05       # 比例系数：建议0.1-2.0，根据系统惯性调整
        self.ki = 0.002      # 积分系数：建议0.01-0.5，用于消除静差
        self.kd = 0.0          # 微分系数：建议0.001-0.2，抑制超调
        
        # ====================== 限幅参数配置 ======================
        self.integral_limit = 0.2  # 积分限幅：防止积分饱和
        self.max_out = 0.523     # 输出限幅：30度弧度制
        
        # ====================== 微分先行参数 ======================
        # 无需额外参数，直接对测量值微分
        
        # ====================== 状态变量初始化 ======================
        self.ref = 0.0            # 目标值（参考输入）
        self.measure = 0.0        # 当前测量值（如编码器速度/位置）
        self.last_measure = 0.0   # 上一时刻测量值（用于微分计算）
        self.err = 0.0            # 当前误差（ref - measure）
        self.last_err = 0.0       # 上一时刻误差
        self.iout = 0.0           # 积分项累加值
        self.iterm = 0.0          # 本次积分增量
        self.dout = 0.0           # 微分项输出
        self.output = 0.0         # 最终控制输出
        
        # ====================== 优化功能选择（位掩码）=====================
        # 0x14 = 0b00010100：启用积分限幅（0x10）和微分先行（0x04）
        self.improve = 0x14       
        self.dead_band = 0.1      # 死区：误差小于此值时停止调节（单位与测量值一致）
        
        # ====================== 时间管理 ======================
        self.dt = 0.1           # 控制周期（秒），建议0.01-0.1s
        self.last_time = time.time()
        self.last_process_time = time.time()
        self.process_interval = 0.1  # 处理间隔，单位为秒
        
        # ====================== ROS通信接口 ======================
        # 订阅目标值（如路径规划的速度指令）
        self.setpoint_sub = self.create_subscription(
            Float64,
            'setpoint',
            self.setpoint_callback,
            10
        )
        
        # 订阅测量值
        self.euler_sub = self.create_subscription(
            EulerOnly,  # 替换为实际消息类型
            'euler_only',
            self.euler_callback,
            10
        )

        
        # 发布控制输出（如电机PWM信号）
        self.output_pub = self.create_publisher(
            Twist,
            'cmd_vel',
            10
        )
        
        # 创建定时器（按控制周期触发计算）
        self.timer = self.create_timer(self.dt, self.timer_callback)
        
        self.get_logger().info("PID控制器节点启动，等待接收数据...")
    
    #更新目标值
    def setpoint_callback(self, msg):
        """目标值订阅回调：更新控制目标"""
        self.ref = msg.data
        #self.get_logger().info(f"接收目标值：{self.ref}")
    
    #定时回调，控制小车按指定方向前进
    def timer_callback(self):
        """定时器回调：执行PID计算并发布输出"""
        current_time = time.time()
        self.dt = current_time - self.last_time  # 计算实际周期
        self.last_time = current_time
        
        self.output = self.pid_calculate(self.measure, self.ref)
        self.publish_output()
    
    #更新imu的数据
    def euler_callback(self, msg):
        current_time = time.time()
        # 仅当距离上一次处理超过指定时间间隔时才处理新数据
        if current_time - self.last_process_time >= self.process_interval:
            self.last_measure = self.measure
            self.measure = msg.euler.yaw
            self.last_process_time = current_time
           # self.get_logger().info(f"处理欧拉角数据: {self.measure}")

    #进行PID计算，通过差速控制小车的前进方向
    def pid_calculate(self, measure, ref):
        """核心PID计算函数"""
        self.ref = ref
        self.measure = measure
        self.calculate_error()
        
        if abs(self.err) > self.dead_band:
            self.calculate_base_pid()
            self.apply_improvements()
            self.limit_output()
        else:
            self.reset_integral()
        self.save_states()
        return self.output
    
    def calculate_error(self):
        """计算误差及历史误差"""
        self.last_err = self.err
        self.err = self.ref - self.measure
    
    def calculate_base_pid(self):
        """计算基础PID项（比例、积分、微分）"""
        self.pout = self.kp * self.err
        self.iterm = self.ki * self.err * self.dt  # 基础积分项
        self.dout = self.kd * (self.err - self.last_err) / self.dt  # 基础微分项
    
    def apply_improvements(self):
        """应用优化功能"""
        # 微分先行：仅对测量值求微分
        if self.improve & 0x04:
            self.dout = self.kd * (self.last_measure - self.measure) / self.dt
        
        # 积分限幅
        if self.improve & 0x10:
            self.limit_integral()
        
        # 累加积分项
        self.iout += self.iterm
    
    def limit_integral(self):
        """积分限幅逻辑"""
        temp_iout = self.iout + self.iterm
        temp_output = self.pout + temp_iout + self.dout
        
        # 当输出将饱和且积分仍在错误方向累积时，清除当前积分项
        if abs(temp_output) > self.max_out and self.err * self.iout > 0:
            self.iterm = 0
        
        # 限制积分项在合理范围
        if temp_iout > self.integral_limit:
            self.iout = self.integral_limit
            self.iterm = 0
        elif temp_iout < -self.integral_limit:
            self.iout = -self.integral_limit
            self.iterm = 0
    
    def limit_output(self):
        """输出限幅逻辑"""
        self.output = self.pout + self.iout + self.dout
        self.output = max(min(self.output, self.max_out), -self.max_out)
    
    def reset_integral(self):
        """死区内清除积分项"""
        self.iout = 0.0
        self.iterm = 0.0
        self.output = 0.0
    
    def save_states(self):
        """保存当前状态供下一周期使用"""
        self.last_measure = self.measure
        self.last_err = self.err
    
    def publish_output(self):
        """发布控制输出"""
        msg = Twist()
        msg.linear.x = -0.5      #想改变小车直线行走速度，在这里改
        msg.angular.z = self.output
        self.output_pub.publish(msg)
        self.get_logger().info(f"发布转向输出：{self.output:.2f} 转向积分输出{self.iout:.2f}")

def main(args=None):
    rclpy.init(args=args)
    node = PIDControllerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
